package fem2;

import inf.jlinalg.Array2DMatrix;
import inf.jlinalg.BLAM;
import inf.jlinalg.IMatrix;

import java.util.Arrays;

import junit.framework.Assert;
import math2.ConstantFunctionRnToR;
import math2.FunctionBasisOnRnToR;
import math2.FunctionRnToR;

import org.junit.Test;

import fem2.analysis.Analysis;
import fem2.element.StructuralElement;
import fem2.enu.EchoLevelType;
import fem2.enu.MeshPartType;
import fem2.enu.State;
import fem2.level_set.Crack;
import fem2.level_set.LevelSet;
import fem2.material.StVenantKirchhoffMM;
import fem2.observer.GidDisplacementObserver;
import fem2.observer.TimeStepObserver;
import fem2.observer.TimingObserver;
import fem2.observer.XfemManager;
import fem2.pre_and_post.GidPostStructural;
import fem2.strategies.NonAdaptiveNewtonRaphsonStrategyForXfem;
import fem2.strategies.Strategy;

public class XfemTest1 extends AbstractStructuralStaticDemo {

	public XfemTest1() {
		projectDir = "/home/hbui/kratos_janosch";
		projectName = "xfemtest1.gid";
		meshFileName = "simple_mesh.msh";
	}

	@Override
	public Mesh createMesh() {
		Mesh mesh = new Mesh();

		Node n1 = mesh.addNode(new Node(0, 0));
		Node n2 = mesh.addNode(new Node(1, 0));
		Node n3 = mesh.addNode(new Node(1, 1));
		Node n4 = mesh.addNode(new Node(0, 1));

		mesh.addFace(new Face(n1, n2, n3, n4));

		return mesh;
	}

	// @Override
	// public Model createConditions(Model m) {
	// Mesh mesh = m.getMesh();
	//
	// Node n1 = mesh.getNode(0);
	// Node n2 = mesh.getNode(1);
	// Node n3 = mesh.getNode(2);
	// Node n4 = mesh.getNode(3);
	//
	// Constraint cx = new Constraint(false, true);
	// Constraint cy = new Constraint(true, false);
	//
	// n1.addConstraint(cx);
	// n1.addConstraint(cy);
	// n2.addConstraint(cy);
	//
	// Edge e34 = mesh.addEdge(new Edge(n3, n4));
	// mesh.setNumberOfFields(2);
	//
	// FunctionRnToR f = new ConstantFunctionRnToR(2.0);
	// Load l = new DistributedLoad(e34, null, f);
	// m.addLoad(l);
	//
	// return m;
	// }

	@Override
	public Model createConditions(Model m) {
		Mesh mesh = m.getMesh();

		Node n1 = mesh.getNode(0);
		Node n2 = mesh.getNode(1);
		Node n3 = mesh.getNode(2);
		Node n4 = mesh.getNode(3);

		Constraint cx = new Constraint(false, true);
		Constraint cy = new Constraint(true, false);

		n3.addConstraint(cx);
		n3.addConstraint(cy);
		n2.addConstraint(cx);

		FunctionRnToR f1 = new ConstantFunctionRnToR(2.0);
		FunctionRnToR f2 = new ConstantFunctionRnToR(-2.0);
		Face f = mesh.getFace(0);
		Load l1 = new EdgeLoad(f, n1, n2, null, f2);
		Load l2 = new EdgeLoad(f, n3, n4, null, f1);
		Load l3 = new EdgeLoad(f, n4, n1, f2, null);
		m.addLoad(l1);
		m.addLoad(l2);
		m.addLoad(l3);

		return m;
	}

	@Override
	public MaterialModel createMaterial(Model m) {
		double E = 9000;
		double nu = 0.3;
		double t = 1.0;
		State ss = State.PLANE_STRESS;
		MaterialModel mm = new StVenantKirchhoffMM(E, nu, t, 0, ss);
		return mm;
	}

	@Override
	public Element createElement(MeshPart mp, MaterialModel mm) {
		return new StructuralElement(mp, mm);
	}

	@Test
	public void testLevelSet() {
		Model m = createModel();
		Mesh mesh = m.getMesh();

		double[] tip_start = new double[] { 0.0, 0.5 };
		double[] tip_delta = new double[] { 0.5, 0.0 };
		Crack c = new Crack(m, tip_start, tip_delta);
		// XfemManager x = new XfemManager(m);
		// x.addCrack(c);

		LevelSet psi = c.getLevelSet("psi");
		LevelSet phi = c.getLevelSet("phi");

		double[] expected_psi = new double[] { -0.5, -0.5, 0.5, 0.5 };
		for (int i = 0; i < mesh.countNodes(); i++) {
			double v = psi.valueAt(mesh.getNode(i));
			Assert.assertEquals(expected_psi[i], v);
		}

		double[] expected_phi = new double[] { -0.5, 0.5, 0.5, -0.5 };
		for (int i = 0; i < mesh.countNodes(); i++) {
			double v = phi.valueAt(mesh.getNode(i));
			Assert.assertEquals(expected_phi[i], v);
		}
	}

	public double crackBasis1(double r, double theta) {
		return Math.sqrt(r) * Math.sin(theta / 2);
	}

	public double crackBasis2(double r, double theta) {
		return Math.sqrt(r) * Math.cos(theta / 2);
	}

	public double crackBasis3(double r, double theta) {
		return Math.sqrt(r) * Math.sin(theta / 2) * Math.sin(theta);
	}

	public double crackBasis4(double r, double theta) {
		return Math.sqrt(r) * Math.cos(theta / 2) * Math.sin(theta);
	}

	@Test
	public void testBasis() {
		Model m = createModel();
		Mesh mesh = m.getMesh();

		double[] tip_start = new double[] { 0.0, 0.5 };
		double[] tip_delta = new double[] { 0.5, 0.0 };

		Crack c = new Crack(m, tip_start, tip_delta);
		XfemManager x = new XfemManager(m);
		x.addCrack(c);

		x.fireStarted();
		x.fireTimeStepStarted();

		Face f = mesh.getFace(0);
		Assert.assertEquals(20, f.countModes());
		FunctionBasisOnRnToR b = f.getBasis();

		// double r = 0.5 * Math.sqrt(2);
		// double pi = Math.PI;
		double[][] tp = new double[][] { { -1, -1 }, { 1, -1 }, { 1, 1 }, { -1, 1 } };
		// double[][] rp = new double[][] { { r, -3 * pi / 4 }, { r, -pi / 4 },
		// { r, pi / 4 },
		// { r, 3 * pi / 4 } };
		//

		// FunctionRnToR N1 = b.getBasisFunction(0);
		FunctionRnToR N1b1 = b.getBasisFunction(1);
		FunctionRnToR N1b2 = b.getBasisFunction(2);
		FunctionRnToR N1b3 = b.getBasisFunction(3);
		FunctionRnToR N1b4 = b.getBasisFunction(4);

		// FunctionRnToR N2 = b.getBasisFunction(5);
		FunctionRnToR N2b1 = b.getBasisFunction(6);
		FunctionRnToR N2b2 = b.getBasisFunction(7);
		FunctionRnToR N2b3 = b.getBasisFunction(8);
		FunctionRnToR N2b4 = b.getBasisFunction(9);

		// FunctionRnToR N3 = b.getBasisFunction(10);
		FunctionRnToR N3b1 = b.getBasisFunction(11);
		FunctionRnToR N3b2 = b.getBasisFunction(12);
		FunctionRnToR N3b3 = b.getBasisFunction(13);
		FunctionRnToR N3b4 = b.getBasisFunction(14);

		// FunctionRnToR N4 = b.getBasisFunction(15);
		FunctionRnToR N4b1 = b.getBasisFunction(16);
		FunctionRnToR N4b2 = b.getBasisFunction(17);
		FunctionRnToR N4b3 = b.getBasisFunction(18);
		FunctionRnToR N4b4 = b.getBasisFunction(19);

		for (int i = 0; i < b.getDimension(); i++) {
			Debugger.watch("---------------");
			for (int j = 0; j < tp.length; j++) {
				Debugger.watch(b.getBasisFunction(i).valueAt(tp[j]));
			}
		}

		for (int i = 0; i < tp.length; i++) {
			// Assert.assertEquals(N1.valueAt(tp[i]) * crackBasis1(rp[i][0],
			// rp[i][1]),
			// N1b1.valueAt(tp[i]));
			// Assert.assertEquals(N1.valueAt(tp[i]) * crackBasis2(rp[i][0],
			// rp[i][1]),
			// N1b2.valueAt(tp[i]));
			// Assert.assertEquals(N1.valueAt(tp[i]) * crackBasis3(rp[i][0],
			// rp[i][1]),
			// N1b3.valueAt(tp[i]));
			// Assert.assertEquals(N1.valueAt(tp[i]) * crackBasis4(rp[i][0],
			// rp[i][1]),
			// N1b4.valueAt(tp[i]));
			//
			// Assert.assertEquals(N2.valueAt(tp[i]) * crackBasis1(rp[i][0],
			// rp[i][1]),
			// N2b1.valueAt(tp[i]));
			// Assert.assertEquals(N2.valueAt(tp[i]) * crackBasis2(rp[i][0],
			// rp[i][1]),
			// N2b2.valueAt(tp[i]));
			// Assert.assertEquals(N2.valueAt(tp[i]) * crackBasis3(rp[i][0],
			// rp[i][1]),
			// N2b3.valueAt(tp[i]));
			// Assert.assertEquals(N2.valueAt(tp[i]) * crackBasis4(rp[i][0],
			// rp[i][1]),
			// N2b4.valueAt(tp[i]));
			//
			// Assert.assertEquals(N3.valueAt(tp[i]) * crackBasis1(rp[i][0],
			// rp[i][1]),
			// N3b1.valueAt(tp[i]));
			// Assert.assertEquals(N3.valueAt(tp[i]) * crackBasis2(rp[i][0],
			// rp[i][1]),
			// N3b2.valueAt(tp[i]));
			// Assert.assertEquals(N3.valueAt(tp[i]) * crackBasis3(rp[i][0],
			// rp[i][1]),
			// N3b3.valueAt(tp[i]));
			// Assert.assertEquals(N3.valueAt(tp[i]) * crackBasis4(rp[i][0],
			// rp[i][1]),
			// N3b4.valueAt(tp[i]));
			//
			// Assert.assertEquals(N4.valueAt(tp[i]) * crackBasis1(rp[i][0],
			// rp[i][1]),
			// N4b1.valueAt(tp[i]));
			// Assert.assertEquals(N4.valueAt(tp[i]) * crackBasis2(rp[i][0],
			// rp[i][1]),
			// N4b2.valueAt(tp[i]));
			// Assert.assertEquals(N4.valueAt(tp[i]) * crackBasis3(rp[i][0],
			// rp[i][1]),
			// N4b3.valueAt(tp[i]));
			// Assert.assertEquals(N4.valueAt(tp[i]) * crackBasis4(rp[i][0],
			// rp[i][1]),
			// N4b4.valueAt(tp[i]));

			Assert.assertEquals(0.0, N1b1.valueAt(tp[i]), 1e-10);
			Assert.assertEquals(0.0, N1b2.valueAt(tp[i]), 1e-10);
			Assert.assertEquals(0.0, N1b3.valueAt(tp[i]), 1e-10);
			Assert.assertEquals(0.0, N1b4.valueAt(tp[i]), 1e-10);

			Assert.assertEquals(0.0, N2b1.valueAt(tp[i]), 1e-10);
			Assert.assertEquals(0.0, N2b2.valueAt(tp[i]), 1e-10);
			Assert.assertEquals(0.0, N2b3.valueAt(tp[i]), 1e-10);
			Assert.assertEquals(0.0, N2b4.valueAt(tp[i]), 1e-10);

			Assert.assertEquals(0.0, N3b1.valueAt(tp[i]), 1e-10);
			Assert.assertEquals(0.0, N3b2.valueAt(tp[i]), 1e-10);
			Assert.assertEquals(0.0, N3b3.valueAt(tp[i]), 1e-10);
			Assert.assertEquals(0.0, N3b4.valueAt(tp[i]), 1e-10);

			Assert.assertEquals(0.0, N4b1.valueAt(tp[i]), 1e-10);
			Assert.assertEquals(0.0, N4b2.valueAt(tp[i]), 1e-10);
			Assert.assertEquals(0.0, N4b3.valueAt(tp[i]), 1e-10);
			Assert.assertEquals(0.0, N4b4.valueAt(tp[i]), 1e-10);
		}

	}

	@Test
	public void testBasis2() {
		Model m = createModel();
		Mesh mesh = m.getMesh();

		double[] tip_start = new double[] { 0.0, 0.5 };
		double[] tip_delta = new double[] { 0.5, 0.0 };

		Crack c = new Crack(m, tip_start, tip_delta);
		XfemManager x = new XfemManager(m);
		x.addCrack(c);

		x.fireStarted();
		x.fireTimeStepStarted();

		Face f = mesh.getFace(0);
		Assert.assertEquals(20, f.countModes());
		FunctionBasisOnRnToR b = f.getBasis();

		// FunctionRnToR N1 = b.getBasisFunction(0);
		FunctionRnToR N1b1 = b.getBasisFunction(1);
		FunctionRnToR N1b2 = b.getBasisFunction(2);
		FunctionRnToR N1b3 = b.getBasisFunction(3);
		FunctionRnToR N1b4 = b.getBasisFunction(4);

		// // FunctionRnToR N2 = b.getBasisFunction(5);
		// FunctionRnToR N2b1 = b.getBasisFunction(6);
		// FunctionRnToR N2b2 = b.getBasisFunction(7);
		// FunctionRnToR N2b3 = b.getBasisFunction(8);
		// FunctionRnToR N2b4 = b.getBasisFunction(9);
		//
		// // FunctionRnToR N3 = b.getBasisFunction(10);
		// FunctionRnToR N3b1 = b.getBasisFunction(11);
		// FunctionRnToR N3b2 = b.getBasisFunction(12);
		// FunctionRnToR N3b3 = b.getBasisFunction(13);
		// FunctionRnToR N3b4 = b.getBasisFunction(14);
		//
		// // FunctionRnToR N4 = b.getBasisFunction(15);
		// FunctionRnToR N4b1 = b.getBasisFunction(16);
		// FunctionRnToR N4b2 = b.getBasisFunction(17);
		// FunctionRnToR N4b3 = b.getBasisFunction(18);
		// FunctionRnToR N4b4 = b.getBasisFunction(19);

		double xi = -0.202573014646912;
		double eta = -0.101286507323456;

		Debugger.watch(N1b1.gradientAt(xi, eta));
		Debugger.watch(N1b2.gradientAt(xi, eta));
		Debugger.watch(N1b3.gradientAt(xi, eta));
		Debugger.watch(N1b4.gradientAt(xi, eta));

	}

	@Test
	public void testBmatrix() {
		Model m = createModel();
		Mesh mesh = m.getMesh();

		double[] tip_start = new double[] { 0.0, 0.5 };
		double[] tip_delta = new double[] { 0.5, 0.0 };

		Crack c = new Crack(m, tip_start, tip_delta);
		XfemManager x = new XfemManager(m);
		x.addCrack(c);

		x.fireStarted();
		x.fireTimeStepStarted();

		MaterialModel mm = createMaterial(m);

		Face f = mesh.getFace(0);
		StructuralElement e = new StructuralElement(f, mm);
		IMatrix B = new Array2DMatrix(3, 40);

		double xi = -0.202573014646912;
		double eta = -0.101286507323456;

		e.computeB(f, B, xi, eta);
		// e.computeB(f, B, 0.0, 0.0); singular at crack tip

		Debugger.watch("B = ", B);
	}

	@Test
	public void testEnumeration() {
		Model m = createModel();
		Mesh mesh = m.getMesh();

		double[] tip_start = new double[] { 0.0, 0.5 };
		double[] tip_delta = new double[] { 0.5, 0.0 };

		Crack c = new Crack(m, tip_start, tip_delta);
		XfemManager x = new XfemManager(m);
		x.addCrack(c);

		x.fireStarted();
		x.fireTimeStepStarted();

		for (int i = 0; i < mesh.countNodes(); i++) {
			Debugger.watch("", mesh.getNode(i).getDofNumbers());
		}
	}

	@Test
	public void testKe() {
		Model m = createModel();

		double[] tip_start = new double[] { 0.0, 0.5 };
		double[] tip_delta = new double[] { 0.5, 0.0 };

		Crack c = new Crack(m, tip_start, tip_delta);
		XfemManager x = new XfemManager(m);
		x.addCrack(c);

		x.fireStarted();
		x.fireTimeStepStarted();

		m.Initialize();

		double[] x0 = new double[m.getSize()];

		Face f = m.getMesh().getFace(0);
		IMatrix ke = new Array2DMatrix(f.countDofs(), f.countDofs());
		m.setX(0.0, x0);
		m.getElement(0).computeKte(ke);
		Debugger.watch(ke);

	}

	@Test
	public void testKt() {
		Model m = createModel();

		double[] tip_start = new double[] { 0.0, 0.5 };
		double[] tip_delta = new double[] { 0.5, 0.0 };

		Crack c = new Crack(m, tip_start, tip_delta);
		XfemManager x = new XfemManager(m);
		x.addCrack(c);

		x.fireStarted();
		x.fireTimeStepStarted();

		m.Initialize();

		double[] x0 = new double[m.getSize()];

		IMatrix kt = new Array2DMatrix(m.getSize(), m.getSize());
		m.setX(0.0, x0);
		m.assembleKt(kt);
		Debugger.watch(kt);

		// ArrayVector ri = new ArrayVector(m.getSize());
		// m.assembleRi(ri.getValues());
		// Debugger.watch(ri);

		double[] x1 = new double[m.getSize()];
		Arrays.fill(x1, 1.0);
		m.setX(0.0, x1);
		BLAM.zero(kt);
		m.assembleKt(kt);
		Debugger.watch(kt);
	}

	@Test
	public void testEdgeLoad() {
		Model m = createModel();

		double[] tip_start = new double[] { 0.0, 0.5 };
		double[] tip_delta = new double[] { 0.5, 0.0 };

		Crack c = new Crack(m, tip_start, tip_delta);
		XfemManager x = new XfemManager(m);
		x.addCrack(c);

		x.fireStarted();
		x.fireTimeStepStarted();

		m.Initialize();

		Face f = m.getMesh().getFace(0);
		FunctionBasisOnRnToR b = f.getBasis();

		FunctionRnToR N3b1 = b.getBasisFunction(11);
		int p = 5;
		double load = 0.0;
		int ng = IntegrationManager.getInstance().getNumIntegrationPoints(MeshPartType.LINE2, p);
		double[] xi1 = new double[] { -1, 1 };
		double[] xi2 = new double[] { 1, 1 };
		for (int i = 0; i < ng; i++) {
			double w = IntegrationManager.getInstance().getIntegrationWeight(MeshPartType.LINE2, p,
					i);
			double[] xi = IntegrationManager.getInstance().getIntegrationPoint(MeshPartType.LINE2,
					p, i);
			double N1 = (1 - xi[0]) / 2;
			double N2 = (1 + xi[0]) / 2;
			double[] xiRef = new double[] { N1 * xi1[0] + N2 * xi2[0], N1 * xi1[1] + N2 * xi2[1] };

			load += 2 * N3b1.valueAt(xiRef) * w * 0.5;
		}
		Debugger.watch(load);

	}

	@Override
	protected void addFundamentalObservers(Model m, Strategy s, TimeStepObserver o) throws Error {
		if (meshFileName == null) {
			throw new Error("mesh file name was not set");
		}

		/*
		 * clean all previous post files
		 */
		String meshName = StringUtilities.getFileName(meshFileName);

		GidPostStructural post = new GidPostStructural(projectDir, projectName, meshName);
		post.clearAllPostFiles();

		Observer do1 = new GidDisplacementObserver(m, o, projectDir, projectName, meshName);

		s.addObserver(do1);
		s.addObserver(new TimingObserver());
	}

	@Override
	public void run() {
		Model m = createModel();

		Topology to = new Topology(m);
		m.getMesh().setTopology(to);

		Analysis an = new Analysis(m);
		an.setStrategy(new NonAdaptiveNewtonRaphsonStrategyForXfem(m, 1));
		Strategy s = an.getStrategy();
		// s.setSolverType(SolverType.BAITSCH_SOLVER_GENERIC);
		s.setEchoLevel(EchoLevelType.OUTPUT, EchoLevelType.AUX1);

		double[] tip_start = new double[] { 0.0, 0.5 };
		double[] tip_delta = new double[] { 0.5, 0.0 };

		Crack c = new Crack(m, tip_start, tip_delta);
		c.setJIntegralRadius(1.0);
		c.setPropagationStep(0.1);

		XfemManager x = new XfemManager(m);

		x.addCrack(c);

		s.addObserver(x);

		addFundamentalObservers(m, s, new TimeStepObserver());

		an.run();

		postProcess(m);

		Node n0 = m.getMesh().getNode(0);
		Debugger.watch(n0.getUHatLocal());

		Face f = m.getMesh().getFace(0);
		double[] uh = f.getUHatLocal();
		FunctionBasisOnRnToR basis = f.getBasis();
		double[] u0 = new double[2];
		for (int i = 0; i < basis.getDimension(); i++) {
			u0[0] += basis.getBasisFunction(i).valueAt(-1, -1) * uh[2 * i];
			u0[1] += basis.getBasisFunction(i).valueAt(-1, -1) * uh[2 * i + 1];
		}
		Debugger.watch(u0);

		Debugger.warn("Analysis completed");
	}

	public static void main(String[] args) {
		XfemTest1 demo = new XfemTest1();

		demo.run();

	}
}
